#ifndef VOXEL_INTERPOLATION_H
#define VOXEL_INTERPOLATION_H

#include<iostream>

#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>

namespace py = pybind11;

static constexpr float EPS = std::numeric_limits<float>::epsilon();

struct Point
{
  float x{0.0f};
  float y{0.0f};
};

int voxel_encode(float const* gt_lines_pts_ptr,
                 std::uint16_t const& gt_lines_num,
                 std::uint16_t const& gt_lines_length,
                 float const* voxels_size,
                 float const* pts_range,
                 std::uint16_t const& max_dense_pts_size,
                 std::uint16_t const& dense_pts_length,
                 std::uint16_t const& voxels_size_dim,
                 float* dense_pts_ptr);

std::uint16_t process_one_line(float const* gt_lines_pts_ptr,
                               std::uint16_t const& start_i,
                               std::uint16_t const& end_i,
                               std::uint16_t const& gt_lines_length,
                               std::array<float, 3> const& voxels_size_,
                               std::array<float, 6> const& pts_range_,
                               std::array<Point, 2048>& ordered_pts);

std::uint16_t fill_output(std::array<Point, 2048> const& ordered_pts,
                          std::uint16_t const& ordered_pts_filled_size,
                          float& id,
                          float const& cls,
                          float const& type,
                          std::uint16_t& filled_size,
                          std::array<float, 3> const& max_xyz_,
                          std::uint16_t const& max_dense_pts_size,
                          std::uint16_t const& dense_pts_length,
                          float* dense_pts_ptr);

std::uint16_t sort_cross_pts(std::array<Point, 2048>& ordered_pts,
                             std::array<Point, 2048> const& x_pts,
                             std::array<Point, 2048> const& y_pts,
                             std::uint16_t ordered_pts_filled_size,
                             std::uint16_t const& x_filled_size,
                             std::uint16_t const& y_filled_size);

inline bool get_cross_pts(Point const& p0,
                          Point const& p1,
                          Point const& q0,
                          Point const& q1,
                          Point& out) {
  double a0 = p0.y - p1.y;
  double b0 = p1.x - p0.x;
  double c0 = p0.x * p1.y - p1.x * p0.y;

  double a1 = q0.y - q1.y;
  double b1 = q1.x - q0.x;
  double c1 = q0.x * q1.y - q1.x * q0.y;


  double D = a0 * b1 - a1 * b0;
  if (std::fabs(D) < EPS){
    return false;
  }

  out.x = static_cast<float>((b0 * c1 - b1 * c0) / D);
  out.y = static_cast<float>((a1 * c0 - a0 * c1) / D);
  return true;
};

inline float get_distance_square(Point const& a, Point const& b){
  return std::pow((a.x - b.x), 2) + std::pow((a.y - b.y), 2);
};

inline float get_clockwise(float const& x, float const &y) {
  return  ((std::fabs(x-1) < EPS) || (std::fabs(y) < EPS)) ? 4-(x+y) : (x+y);
};

int gt_lines_voxel_interpolation(py::array_t<float> gt_lines_pts,
                                 py::array_t<float> voxels_size,
                                 py::array_t<float> pts_range,
                                 int max_dense_pts_size,
                                 py::array_t<float> dense_pts);

#endif
